/*
*  Copyright 2018, Sebastian Pütz
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*  1. Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*
*  2. Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*
*  3. Neither the name of the copyright holder nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  mbf_costmap_core.h
*
*  author: Sebastian Pütz <spuetz@uni-osnabrueck.de>
*
*/

#ifndef MBF_COSTMAP_CORE__COSTMAP_CONTROLLER_H_
#define MBF_COSTMAP_CORE__COSTMAP_CONTROLLER_H_

#include <mbf_abstract_core/abstract_controller.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <mbf_utility/types.h>

namespace mbf_costmap_core
{
/**
 * @class LocalPlanner
 * @brief Provides an interface for local planners used in navigation.
 * All local planners written to work as MBF plugins must adhere to this interface.
 * Alternatively, this class can also operate as a wrapper for old API nav_core-based plugins,
 * providing backward compatibility.
 */
class CostmapController : public mbf_abstract_core::AbstractController
{
public:

    typedef boost::shared_ptr<::mbf_costmap_core::CostmapController> Ptr;

    /** 给定机器人的当前位置、方向和速度，计算并发送到底盘的速度命令。
     * @brief Given the current position, orientation, and velocity of the robot,
     *        compute velocity commands to send to the base.
     * @param pose the current pose of the robot. 机器人的当前姿态。
     * @param velocity the current velocity of the robot.  机器人当前的速度。
     * @param cmd_vel Will be filled with the velocity command to be passed to the robot base. 将填充要传递给机器人基座的速度命令。
     * @param message Optional more detailed outcome as a string 作为字符串可选的更详细的结果
     * @return Result code as described on ExePath action result: ExePath操作结果中描述的结果代码:
     *         SUCCESS         = 0
     *         1..9 are reserved as plugin specific non-error results 1 . .9被保留为插件特定的非错误结果
     *         FAILURE         = 100   Unspecified failure, only used for old, non-mfb_core based plugins
     *         CANCELED        = 101   取消了
     *         NO_VALID_CMD    = 102   无效的CMD命令
     *         PAT_EXCEEDED    = 103   超时
     *         COLLISION       = 104   碰撞
     *         OSCILLATION     = 105   振荡
     *         ROBOT_STUCK     = 106   卡住了
     *         MISSED_GOAL     = 107   错过了的目标
     *         MISSED_PATH     = 108   错过了路径
     *         BLOCKED_PATH    = 109   挡住了道路
     *         INVALID_PATH    = 110   无效的路径
     *         TF_ERROR        = 111   TF错误
     *         NOT_INITIALIZED = 112   没有初始化
     *         INVALID_PLUGIN  = 113   无效的插件
     *         INTERNAL_ERROR  = 114   内部错误
     *         121..149 are reserved as plugin specific errors  121..149保留为插件特定的错误
     */
    virtual uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped &pose,
                                             const geometry_msgs::TwistStamped &velocity,
                                             geometry_msgs::TwistStamped &cmd_vel,
                                             std::string &message) = 0;

    /** 检查局部规划器是否在公差范围内实现了目标位姿
     * @brief Check if the goal pose has been achieved by the local planner within tolerance limits
     * @remark New on MBF API
     * @param xy_tolerance Distance tolerance in meters
     * @param yaw_tolerance Heading tolerance in radians
     * @return True if achieved, false otherwise
     */
    virtual bool isGoalReached(double xy_tolerance, double yaw_tolerance) = 0;

    /**
     * @brief  Set the plan that the local planner is following
     * @param plan The plan to pass to the local planner
     * @return True if the plan was updated successfully, false otherwise
     */
    virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;

    /**
     * @brief Requests the planner to cancel, e.g. if it takes too much time
     * @remark New on MBF API
     * @return True if a cancel has been successfully requested, false if not implemented.
     */
    virtual bool cancel() = 0;

    /**
     * @brief Constructs the local planner
     * @param name The name to give this instance of the local planner
     * @param tf A pointer to a transform listener
     * @param costmap_ros The cost map to use for assigning costs to local plans
     */
    virtual void initialize(std::string name, ::TF *tf, costmap_2d::Costmap2DROS *costmap_ros) = 0;

    /**
     * @brief  Virtual destructor for the interface
     */
    virtual ~CostmapController() {}

protected:
    CostmapController() {}

};
}  /* namespace mbf_costmap_core */

#endif  /* MBF_COSTMAP_CORE__COSTMAP_CONTROLLER_H_ */
